/*
 * Copyright (c) 2018 - 2019, AEye, Inc.
 * All rights reserved.
 */
#include "spdlog/spdlog.h"

#include "HawkeyeFrame.hpp"
#include "SensorDev.hpp"
#include "SensorReader.hpp"
#include "SensorCommander.hpp"

using aeye::GainChannel;
using aeye::PixelMode;



SensorDev::SensorDev(aeye::SensorConfig sensor_config, aeye::DataPortConfig data_ports,
                     HawkeyePortConfig hawkeye_ports, std::string net_interface,
                     std::string sensor_ip, std::string sensor_name) :
  Sensor(sensor_config, data_ports, std::move(net_interface), std::move(sensor_ip), std::move(sensor_name)),
  _hawkEyePort(hawkeye_ports.client), _serverHawkEyePort(aeye::DEFAULT_SERVER_HAWKEYE_PORT) {

  sensorReader = std::make_unique<SensorReader>(sensorIp, rangePort, diagnosticPort, _hawkEyePort,
                                                name, false);
}

bool SensorDev::isSelected() const {
  return _selected;
}

void SensorDev::setSelected(bool selected) {
  _selected = selected;
}

void SensorDev::start() {
  if (!pcapModeOn && _isManualMode) {
    throw std::logic_error("Can't start. Sensor in manual mode! \nPlease turn off manual mode.");
  }
  sensingOn = true;
  sensorReader->start();
  if (!pcapModeOn) {
    sensorCommander->cmdStart(clientIp, rangePort);
  }
}

void SensorDev::writeScannerRegister(uint32_t addr, uint32_t value) {
  try {
    sensorCommander->writeScannerRegister(addr, value);
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->writeScannerRegister() raised exception: {}.", e.what());
    throw;
  }
}

uint32_t SensorDev::readScannerRegister(uint32_t addr) {
  try {
    return sensorCommander->readScannerRegister(addr);
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->readScannerRegister() raised exception: {}.", e.what());
    throw;
  }
}

void SensorDev::writeReceiverRegister(uint32_t addr, uint32_t value) {
  try {
    sensorCommander->writeReceiverRegister(addr, value);
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->writeReceiverRegister() raised exception: {}.", e.what());
    throw;
  }
}

uint32_t SensorDev::readReceiverRegister(uint32_t addr) {
  try {
    return sensorCommander->readReceiverRegister(addr);
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->readReceiverRegister() raised exception: {}.", e.what());
    throw;
  }
}

// HawkEye
void SensorDev::sendHawkEyePoint(bool enable, uint16_t row, uint16_t col){
  sensorCommander->setHawkEyePoint(enable, row, col);
}
void SensorDev::setHawkEyeCallback(const std::function<void(aeye::HawkeyeFrame&&)>& processCallback) {
   sensorReader->setHawkeyeCallback(processCallback);
}

void SensorDev::startHawkEye() {
  if (!isPcapMode()){
    toggleHawkEye(true, clientIp, _hawkEyePort);
  }
  sensorReader->startHawkeye();
  hawkeyeOn = true;
}

void SensorDev::stopHawkEye() {
  if (!isPcapMode()){
    toggleHawkEye(false, clientIp, _hawkEyePort);
  }
  sensorReader->stopHawkeye();
  hawkeyeOn = false;
}

void SensorDev::toggleARxSweepMode(bool enable, GainChannel gain_channel, PixelMode pixel_mode, uint16_t colSize,
                                   uint16_t rowSize) {
  ensureHawkeyeOn(enable);
  sensorCommander->toggleARxSweepMode(enable, clientIp, _hawkEyePort, gain_channel, pixel_mode, colSize, rowSize);
}

void SensorDev::toggleARxStaticMode(bool enable, PixelMode pixel_mode, uint32_t row, uint32_t col){
  ensureHawkeyeOn(enable);
  sensorCommander->toggleARxStaticMode(enable, clientIp, _hawkEyePort, pixel_mode, row, col);
}

void SensorDev::generateARxLUTV2(aeye::RxFOV rxFov, aeye::PixelMode pixelMode, uint8_t rxWindowSize) {
  sensorCommander->generateARxLUTV2(rxFov, pixelMode, rxWindowSize);
}

void SensorDev::getGenerateARxLutV2Status(bool &isRunning,
                                          uint8_t &calIterations,
                                          uint8_t &startScore,
                                          uint64_t &endScore,
                                          float &ratio) {
  sensorCommander->getGenerateARxLutV2Status(isRunning, calIterations, startScore, endScore, ratio);
}

void SensorDev::getHawkeyeBuffer(uint32_t frameCount) {
  ensureHawkeyeOn();
  sensorCommander->getHawkeyeBuffer(frameCount);
}

void SensorDev::setARxPixel(uint8_t col, uint8_t row, uint8_t pixelMode) {
  ensureHawkeyeOn();
  sensorCommander->setARxPixel(col, row, pixelMode);
}

void SensorDev::toggleHawkEye(bool enable, const std::string& clientIPAddress, uint16_t port) {
  sensorCommander->toggleHawkEye(enable, clientIPAddress, port);
}

void SensorDev::toggleScanningMode(bool scanningMode) {
  ensureHawkeyeOn(scanningMode);
  sensorCommander->toggleScanningMode(scanningMode);
}

void SensorDev::ensureHawkeyeOn() {
  if (hawkeyeOn) return;
  spdlog::info("Starting HawkEye.");
  sensorReader->startHawkeye();
  hawkeyeOn = true;
}

void SensorDev::ensureHawkeyeOn(bool enable) {
  if (hawkeyeOn) return;
  else if (enable) {
    sensorReader->startHawkeye();
    hawkeyeOn = true;
  } else {
    sensorReader->stopHawkeye();
    hawkeyeOn = false;
  }
}

void SensorDev::toggleManualMode(bool manualMode) {
  if (manualMode) {
    writeScannerRegister(80, 0b010); // Switch Scanner to manual mode (Firing from registers)
    writeScannerRegister(79, 1);     // FIFO_reset = 1 Holds FIFO reset
  }
  else {
    writeScannerRegister(80, 0); // Reset Scanner FIFO/Reg mode
    writeScannerRegister(79, 0); // FIFO_reset = 0 (disable reset and attaches TX FIFO to laser fire logic)
  }
}

void SensorDev::setLaserEnable(uint8_t enable) {
  try {
    sensorCommander->setLaserEnable(enable);
  } catch (const std::exception &e) {
    spdlog::warn("sensorCommander->setLaserEnable() raised exception: {}.", e.what());
  }
}

void SensorDev::setPoseInPcapMode(aeye::Pose sensor_pose) {
  if (pcapModeOn) { pose = sensor_pose; }
}

void SensorDev::setShotlistNameInPcapMode(const std::string& shotlist_name) {
  if (pcapModeOn) {
    scanPatternFileNames.clear();
    scanPatternFileNames.push_back(shotlist_name);
  }
}

void SensorDev::sendScanZoneParameters(std::vector<aeye::AdvancedZoneScanRegion>& scan_regions) {
  try {
    // Invoke the corresponding command in SensorCommander
    sensorCommander->sendScanZoneParameters(scan_regions);
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->sendScanZoneParameters() raised exception: {}.", e.what());
    throw std::runtime_error(e.what());
  }
}

void SensorDev::killSensorService() {
  try {
    sensorCommander->killSensorService();
  }
  catch (std::exception& e) {
    spdlog::warn("sensorCommander->killSensorService() raised exception: {}.", e.what());
  }
}

bool SensorDev::isHawkeyeOn() const noexcept {
  return hawkeyeOn;
}
uint8_t SensorDev::getScanPatternFlags() const noexcept {
  return scanPatternFlags;
}
uint32_t SensorDev::getScanblockId() const noexcept {
  return scanBlockId;
}
const std::string &SensorDev::getMacAddress() const noexcept {
  return macAddress;
}
uint16_t SensorDev::getHawkEyePort() const noexcept {
  return _hawkEyePort;
}
uint16_t SensorDev::getServerHawkEyePort() const noexcept {
  return _serverHawkEyePort;
}
void SensorDev::setRxFOV(aeye::RxFOV rx_fov) {
  sensorCommander->setRxFOV(rx_fov);
}

void SensorDev::sendLaserPower(float value) {
  try {
    sensorCommander->setLaserPower(value);
    laserPower = value;
  } catch (std::exception &e) {
    spdlog::warn("sensorCommander->setLaserPower() raised exception: {}.", e.what());
  }
}

aeye::HsyncParameters SensorDev::getHsyncParams() {
    aeye::HsyncParameters hsyncParams{};
    aeye::PacketGetHsyncParams hsync_parameter_packet = sensorCommander->getHsyncParameters();
    hsyncParams.avgTemp = hsync_parameter_packet.avg_samples_temp;
    hsyncParams.instTemp = hsync_parameter_packet.inst_temp;
    hsyncParams.np0Capacitor = hsync_parameter_packet.np0_capacitor;
    return hsyncParams;
}